#! /usr/bin/env python
# Forearm camera configuration

PACKAGE='cyphy_CF'
import roslib; roslib.load_manifest(PACKAGE)

from math import pi

from dynamic_reconfigure.msg import SensorLevels
from dynamic_reconfigure.parameter_generator import *

gen = ParameterGenerator()
#       Name                    Type      Reconfiguration level             Description                                                                                      Default    Min   Max
gen.add("Position_x_P",double_t, SensorLevels.RECONFIGURE_STOP,    "Position x P gain. The default value is 8",7.8,0,20)
gen.add("Position_x_I",double_t, SensorLevels.RECONFIGURE_STOP,    "Position x I gain. The default value is 3",0,0,20)
gen.add("Position_x_D",double_t, SensorLevels.RECONFIGURE_STOP,    "Position x D gain. The default value is 5",1.2,0,20)
gen.add("Position_x_I_MAX",double_t, SensorLevels.RECONFIGURE_STOP,    "Position x I MAX. The default value is 5",0,0,30)
gen.add("Position_x_I_MIN",double_t, SensorLevels.RECONFIGURE_STOP,    "Position x I MIN. The default value is -5",0,-30,0)


gen.add("Position_y_P",double_t, SensorLevels.RECONFIGURE_STOP,    "Position y P gain. The default value is 8",7.8,0,20)
gen.add("Position_y_I",double_t, SensorLevels.RECONFIGURE_STOP,    "Position y I gain. The default value is 3",0,0,20)
gen.add("Position_y_D",double_t, SensorLevels.RECONFIGURE_STOP,    "Position y D gain. The default value is 5",1.2,0,20)
gen.add("Position_y_I_MAX",double_t, SensorLevels.RECONFIGURE_STOP,    "Position y I MAX. The default value is 5",0,0,30)
gen.add("Position_y_I_MIN",double_t, SensorLevels.RECONFIGURE_STOP,    "Position y I MIN. The default value is -5",0,-30,0)




gen.add("Velocity_x_P",double_t, SensorLevels.RECONFIGURE_STOP,    "Velocity x P gain. The default value is 22.05",16,0,40)
gen.add("Velocity_x_I",double_t, SensorLevels.RECONFIGURE_STOP,    "Velocity x I gain. The default value is 0",0,0.0,10.0)
gen.add("Velocity_x_D",double_t, SensorLevels.RECONFIGURE_STOP,    "Velocity x D gain. The default value is 0",0,0.0,10.0)

gen.add("Velocity_y_P",double_t, SensorLevels.RECONFIGURE_STOP,    "Velocity y P gain. The default value is 22.05",16,0,40)
gen.add("Velocity_y_I",double_t, SensorLevels.RECONFIGURE_STOP,    "Velocity y I gain. The default value is 0",0,0.0,10.0)
gen.add("Velocity_y_D",double_t, SensorLevels.RECONFIGURE_STOP,    "Velocity y D gain. The default value is 0",0,0.0,10.0)



gen.add("Height_P",double_t, SensorLevels.RECONFIGURE_STOP,    "Height Position P gain. The default value is 25",28.5,0,50)
gen.add("Height_I",double_t, SensorLevels.RECONFIGURE_STOP,    "Height Position I gain. The default value is 2.5",1.5,0,10)
gen.add("Height_D",double_t, SensorLevels.RECONFIGURE_STOP,    "Height Position D gain. The default value is 29.5",36,0,50)
gen.add("Height_I_MAX",double_t, SensorLevels.RECONFIGURE_STOP,    "Height Position I MAX. The default value is 5",19.2,0,30)
gen.add("Height_I_MIN",double_t, SensorLevels.RECONFIGURE_STOP,    "Height Position I MIN. The default value is -5",-19.2,-30,0)


#gen.add("Height_P",double_t, SensorLevels.RECONFIGURE_STOP,    "Height Pos P gain. The default value is 7",7,0.0,35)
#gen.add("Height_I",double_t, SensorLevels.RECONFIGURE_STOP,    "Height Pos I gain. The default value is 2",2,0.0,35)
#gen.add("Height_D",double_t, SensorLevels.RECONFIGURE_STOP,    "Height Pos D gain. The default value is 3",3,0.0,35)

gen.add("Height_vel_P",double_t, SensorLevels.RECONFIGURE_STOP,    "Height Velocity P gain. The default value is 21.7",21.7,0.0,70)
gen.add("Height_vel_I",double_t, SensorLevels.RECONFIGURE_STOP,    "Height Velocity I gain. The default value is 5.6",5.6,0.0,70)
gen.add("Height_vel_D",double_t, SensorLevels.RECONFIGURE_STOP,    "Height Velocity D gain. The default value is 23.8",23.8,0.0,70.0)

gen.add("Height_OFFSET",double_t, SensorLevels.RECONFIGURE_STOP,    "Height OFFSET. The default value is 120",120,0.0,130)


gen.add("Yaw_P",double_t, SensorLevels.RECONFIGURE_STOP,    "Yaw P gain. The default value is 6",6,0.0,15)
gen.add("Yaw_I",double_t, SensorLevels.RECONFIGURE_STOP,    "Yaw I gain. The default value is 1",1,0.0,5)
gen.add("Yaw_D",double_t, SensorLevels.RECONFIGURE_STOP,    "Yaw D gain. The default value is 2",2,0.0,10)

exit(gen.generate(PACKAGE, "cyphy_CF", "onlinegain"))
